#include <iostream>
#include <fstream>
#include <iomanip>
#include "ekf.hpp"
#include "mec.hpp"
#include "nav_base.hpp"

#define D2R 3.1415926535 / 180.0

int notmain(int argc, char** argv){

    ImuModel imumodel;
    imumodel.ARW                          = 0.003 * D2R / 60.0;  
    imumodel.VRW                          = 0.03 / 60.0;   
    imumodel.gyro_bias_std                = 0.027 * D2R / 3600.0;  
    imumodel.gyro_bias_corr_time          = 4 * 3600.0;     
    imumodel.accel_bias_std               = 15 * 1e-5;     
    imumodel.accel_bias_corr_time         = 4 * 3600.0;        

    StatePtr state = std::make_shared<State>();
    state->c_bn << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    state->gyro_bias << 0, 0, 0;
    state->accel_bias << 0, 0, 0;
    state->pos << 0, 0, 0;
    state->vel << 0, 0, 0;
    state->att << 0, 0, 0;
    state->timestamp = 0;

    ImuData imudata;

    Mec mec(state);

    std::ifstream imudata_fs("/home/ghowoght/workspace/meg-odom/data/20211117/imu.txt");
    // std::ifstream imudata_fs("/home/ghowoght/workspace/meg-odom/data/20220116/imu.txt");
    int cnt = 0;
    float gyro_offsets_sum[3]= { 0.0f, 0.0f, 0.0f };
    float accel_offsets_sum[3]= { 0.0f, 0.0f, 0.0f };
    float gyroOffset[3]= { 0.0f, 0.0f, 0.0f };
    float accelOffset[3]= { 0.0f, 0.0f, 0.0f };
    int offset_count = 0;

    while(!imudata_fs.eof()){// && cnt++ < 10){

        imudata_fs  >> imudata.timestamp
                    >> imudata.gyro_rps[0] >> imudata.gyro_rps[1] >> imudata.gyro_rps[2]
                    >> imudata.accel_mpss[0] >> imudata.accel_mpss[1] >> imudata.accel_mpss[2];

        if(!mec.ready)
        {
            gyro_offsets_sum[0] += imudata.gyro_rps[0];
            gyro_offsets_sum[1] += imudata.gyro_rps[1];
            gyro_offsets_sum[2] += imudata.gyro_rps[2];
            accel_offsets_sum[0] += imudata.accel_mpss[0];
            accel_offsets_sum[1] += imudata.accel_mpss[1];
            accel_offsets_sum[2] += imudata.accel_mpss[2];
            offset_count++;
            if(offset_count > 600)
            {
                gyroOffset[0] = gyro_offsets_sum[0]/offset_count;
                gyroOffset[1] = gyro_offsets_sum[1]/offset_count;
                gyroOffset[2] = gyro_offsets_sum[2]/offset_count;
                accelOffset[0] = accel_offsets_sum[0]/offset_count;
                accelOffset[1] = accel_offsets_sum[1]/offset_count;
                accelOffset[2] = accel_offsets_sum[2]/offset_count;
                accelOffset[2]-= 9.81;
                
                offset_count=0;
                gyro_offsets_sum[0]=0;
                gyro_offsets_sum[1]=0;
                gyro_offsets_sum[2]=0;
                mec.ready = true;
            }
            continue;
        }

        
        if(mec.ready){

            imudata.gyro_rps[0] -= - gyroOffset[0];
            imudata.gyro_rps[1] -= - gyroOffset[1];
            imudata.gyro_rps[2] -= - gyroOffset[2];

            imudata.accel_mpss[0] -= accelOffset[0];
            imudata.accel_mpss[1] -= accelOffset[1];
            imudata.accel_mpss[2] -= accelOffset[2];
            
            std::cout << gyroOffset[0] << ", " << gyroOffset[1] << ", " << gyroOffset[2] << std::endl;
            std::cout << accelOffset[0] << ", " << accelOffset[1] << ", " << accelOffset[2] << std::endl;
            

            mec.update(imudata);
        }
    
    }    

    return 0;
}



int main(int argc, char** argv){

    ImuModel imumodel;
    imumodel.ARW                          = 0.00004 * D2R;  
    imumodel.VRW                          = 0.001;   
    imumodel.gyro_bias_std                = 0.027 * D2R / 3600.0;  
    imumodel.gyro_bias_corr_time          = 4 * 3600.0;     
    imumodel.accel_bias_std               = 15 * 1e-3;     
    imumodel.accel_bias_corr_time         = 4 * 3600.0;      
    imumodel.odom_scale_rw                = 0.0001;   

    StatePtr state = std::make_shared<State>();
    state->c_bn << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    Eigen::Vector4d temp;
    temp << 0, 0, 0, 1;
    state->q_bn =Eigen::Quaterniond(temp);

    // state->gyro_bias << 0, 0, 0;
    // state->accel_bias << 0, 0, 0;
    state->gyro_bias << -0.00453676, 0.00562842, 0.002318;
    state->accel_bias << -0.232167, -0.140608, 0.211983;
    

    // state->gyro_bias << -0.336051, 0.172654, -0.173745;
    // state->gyro_bias = state->gyro_bias / 180.0 * 3.1415926535;
    // state->accel_bias << -0.0552951, 0.0713641, -0.0117993;
    // state->gyro_bias << -0.00156756, 0.00410594, 0.00303242;
    // state->accel_bias << -0.0198862, 0.0439074, 0.0117993;
    state->odom_scale_factor = 0;
    state->pos << 0, 0, 0;
    state->vel << 0, 0, 0;
    state->att << 0, 0, 0;
    state->timestamp = 0;

    ImuData imudata;

    EKF ekf;

    DataPtr data_raw = std::make_shared<Data>();
    OdomData odomdata;
    
    std::ifstream imudata_fs("/home/ghowoght/workspace/meg-odom/data/20220116/imu.txt");
    std::ifstream odomdata_fs("/home/ghowoght/workspace/meg-odom/data/20220116/odom.txt");
    // std::ifstream imudata_fs("/home/ghowoght/workspace/meg-odom/data/20211117/imu.txt");
    // std::ifstream odomdata_fs("/home/ghowoght/workspace/meg-odom/data/20211117/odom.txt");

    imudata_fs  >> imudata.timestamp
                >> imudata.gyro_rps[0] >> imudata.gyro_rps[1] >> imudata.gyro_rps[2]
                >> imudata.accel_mpss[0] >> imudata.accel_mpss[1] >> imudata.accel_mpss[2];

    // imudata_fs  >> imudata.timestamp
    //             >> imudata.gyro_rps[1] >> imudata.gyro_rps[0] >> imudata.gyro_rps[2]
    //             >> imudata.accel_mpss[1] >> imudata.accel_mpss[0] >> imudata.accel_mpss[2];
    // for(int i = 0; i < 3; i++){
    //     imudata.gyro_rps[i] = -imudata.gyro_rps[i];
    //     imudata.accel_mpss[i] = -imudata.accel_mpss[i];
    // }
    // imudata.gyro_rps = imudata.gyro_rps / 180.0 * 3.1415926535;
    odomdata_fs >> odomdata.timestamp
                >> odomdata.vel[0] >> odomdata.vel[1] >> odomdata.vel[2];// >> odomdata.vel[2];
    odomdata.vel[1] = odomdata.vel[2] = 0;
    odomdata.valid = true;
    data_raw->imudata = imudata;
    data_raw->odomdata = odomdata;
    ekf.initialize(state, data_raw, imumodel);
    while(!imudata_fs.eof() && imudata.timestamp < 200){// && cnt < 200000){
        imudata_fs  >> imudata.timestamp
                    >> imudata.gyro_rps[0] >> imudata.gyro_rps[1] >> imudata.gyro_rps[2]
                    >> imudata.accel_mpss[0] >> imudata.accel_mpss[1] >> imudata.accel_mpss[2];
        // imudata_fs  >> imudata.timestamp
        //         >> imudata.gyro_rps[1] >> imudata.gyro_rps[0] >> imudata.gyro_rps[2]
        //         >> imudata.accel_mpss[1] >> imudata.accel_mpss[0] >> imudata.accel_mpss[2];
        // for(int i = 0; i < 3; i++){
        //     imudata.gyro_rps[i] = -imudata.gyro_rps[i];
        //     imudata.accel_mpss[i] = -imudata.accel_mpss[i];
        // }
        // imudata.gyro_rps = imudata.gyro_rps / 180.0 * 3.1415926535;

        // if(imudata.timestamp > 10)
        {
            if(!data_raw->odomdata.valid){      
                while(odomdata.timestamp < imudata.timestamp){      
                    odomdata_fs >> odomdata.timestamp
                                >> odomdata.vel[0] >> odomdata.vel[1] >> odomdata.vel[2];// >> odomdata.vel[2];
                    odomdata.vel[1] = odomdata.vel[2] = 0;
                    odomdata.valid = true;
                }
            }     
            data_raw->imudata = imudata;
            data_raw->odomdata = odomdata;
            ekf.Filter(data_raw);
        }
    }    

    return 0;
}